#include <memory>
#include <thread>
#include <cmath>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "fruit_nav/action/navigate_to_pose.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2/LinearMath/Matrix3x3.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"

class FruitNav : public rclcpp::Node
{
public:
  using NavigateToPose = fruit_nav::action::NavigateToPose;
  using GoalHandleNavigateToPose = rclcpp_action::ServerGoalHandle<NavigateToPose>;

  FruitNav() : Node("fruit_nav")
  {
    RCLCPP_INFO(this->get_logger(), "fruit_nav 动作服务器节点已启动");
    
    // 创建动作服务器
    this->action_server_ = rclcpp_action::create_server<NavigateToPose>(
      this,
      "navigate_to_pose",
      std::bind(&FruitNav::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
      std::bind(&FruitNav::handle_cancel, this, std::placeholders::_1),
      std::bind(&FruitNav::handle_accepted, this, std::placeholders::_1));
    
    // 创建TF监听器，从cartographer获取定位信息
    tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
    
    // 发布速度命令
    cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
    
    RCLCPP_INFO(this->get_logger(), "动作服务器 'navigate_to_pose' 已创建，等待目标位姿...");
    RCLCPP_INFO(this->get_logger(), "使用TF监听 'map -> base_link' 变换获取定位信息");
    RCLCPP_INFO(this->get_logger(), "发布速度话题 '/cmd_vel'");
  }

private:
  rclcpp_action::Server<NavigateToPose>::SharedPtr action_server_;
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
  
  // TF相关
  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
  
  // 导航参数
  const double linear_speed_ = 0.5;  // 线速度 (m/s)
  const double angular_speed_initial_ = 0.5; // 初始角速度 (rad/s)
  const double angular_speed_max_ = 0.7; // 最大角速度 (rad/s)
  const double angular_speed_increment_ = 0.05; // 角速度增量 (rad/s)
  const double position_tolerance_ = 0.1; // 位置容差 (m)
  const double angle_tolerance_ = 0.02; // 角度容差 (rad)
  const double angle_deviation_threshold_ = 0.15; // 第二阶段角度偏差阈值 - 超过此值返回阶段1重新调整
  const double correction_angular_gain_ = 2.0; // 矫正时的角速度增益系数
  const double angle_change_threshold_ = 0.01; // 角度变化阈值 (rad) - 低于此值认为角度变化缓慢
  const int stagnation_count_threshold_ = 10; // 停滞次数阈值 - 超过此次数则增加角速度
  
  // 通过TF获取当前位姿
  bool getCurrentPose(double& x, double& y, double& yaw)
  {
    try
    {
      // 从map坐标系查找base_link的位姿
      geometry_msgs::msg::TransformStamped transform = 
        tf_buffer_->lookupTransform("map", "base_link", tf2::TimePointZero);
      
      x = transform.transform.translation.x;
      y = transform.transform.translation.y;
      yaw = getYawFromQuaternion(transform.transform.rotation);
      
      return true;
    }
    catch (tf2::TransformException &ex)
    {
      RCLCPP_WARN(this->get_logger(), "无法获取TF变换: %s", ex.what());
      return false;
    }
  }
  
  // 从四元数获取偏航角
  double getYawFromQuaternion(const geometry_msgs::msg::Quaternion& quat)
  {
    tf2::Quaternion tf_quat;
    tf2::fromMsg(quat, tf_quat);
    double roll, pitch, yaw;
    tf2::Matrix3x3(tf_quat).getRPY(roll, pitch, yaw);
    return yaw;
  }
  
  // 规范化角度到[-pi, pi]
  double normalizeAngle(double angle)
  {
    while (angle > M_PI) angle -= 2.0 * M_PI;
    while (angle < -M_PI) angle += 2.0 * M_PI;
    return angle;
  }
  
  // 自适应角速度控制：初始角速度为0.4，如果角度变化缓慢则逐步增加
  double calculateAdaptiveAngularSpeed(double prev_angle_error, double current_angle_error, 
                                       int& stagnation_count, double& current_angular_speed)
  {
    // 计算角度变化量
    double angle_change = std::abs(current_angle_error) - std::abs(prev_angle_error);
    
    // 如果角度变化小于阈值，认为进展缓慢
    if (std::abs(angle_change) < angle_change_threshold_)
    {
      stagnation_count++;
      
      // 如果停滞次数超过阈值，增加角速度
      if (stagnation_count >= stagnation_count_threshold_)
      {
        current_angular_speed = std::min(current_angular_speed + angular_speed_increment_, 
                                         angular_speed_max_);
        stagnation_count = 0; // 重置计数器
        RCLCPP_INFO(this->get_logger(), 
                    "检测到角度变化缓慢，增加角速度至: %.2f rad/s", 
                    current_angular_speed);
      }
    }
    else
    {
      // 角度变化正常，重置计数器
      stagnation_count = 0;
    }
    
    return current_angular_speed;
  }
  
  // 计算两点之间的距离
  double calculateDistance(double x1, double y1, double x2, double y2)
  {
    return std::sqrt(std::pow(x2 - x1, 2) + std::pow(y2 - y1, 2));
  }
  
  // 计算目标角度
  double calculateTargetAngle(double x1, double y1, double x2, double y2)
  {
    return std::atan2(y2 - y1, x2 - x1);
  }

  // 处理新的目标请求
  rclcpp_action::GoalResponse handle_goal(
    const rclcpp_action::GoalUUID & uuid,
    std::shared_ptr<const NavigateToPose::Goal> goal)
  {
    (void)uuid;
    
    RCLCPP_INFO(this->get_logger(), "收到目标位姿请求:");
    RCLCPP_INFO(this->get_logger(), "  Frame: %s", goal->target_pose.header.frame_id.c_str());
    RCLCPP_INFO(this->get_logger(), "  位置 - x: %.2f, y: %.2f, z: %.2f", 
                goal->target_pose.pose.position.x,
                goal->target_pose.pose.position.y,
                goal->target_pose.pose.position.z);
    RCLCPP_INFO(this->get_logger(), "  姿态 - x: %.2f, y: %.2f, z: %.2f, w: %.2f",
                goal->target_pose.pose.orientation.x,
                goal->target_pose.pose.orientation.y,
                goal->target_pose.pose.orientation.z,
                goal->target_pose.pose.orientation.w);
    
    // 接受所有目标
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }

  // 处理取消请求
  rclcpp_action::CancelResponse handle_cancel(
    const std::shared_ptr<GoalHandleNavigateToPose> goal_handle)
  {
    RCLCPP_INFO(this->get_logger(), "收到取消请求");
    (void)goal_handle;
    return rclcpp_action::CancelResponse::ACCEPT;
  }

  // 处理已接受的目标
  void handle_accepted(const std::shared_ptr<GoalHandleNavigateToPose> goal_handle)
  {
    // 在新线程中执行导航任务，避免阻塞
    std::thread{std::bind(&FruitNav::execute, this, std::placeholders::_1), goal_handle}.detach();
  }

  // 执行导航任务
  void execute(const std::shared_ptr<GoalHandleNavigateToPose> goal_handle)
  {
    RCLCPP_INFO(this->get_logger(), "开始执行导航任务...");
    
    const auto goal = goal_handle->get_goal();
    auto feedback = std::make_shared<NavigateToPose::Feedback>();
    auto result = std::make_shared<NavigateToPose::Result>();
    
    // 等待TF变换可用
    RCLCPP_INFO(this->get_logger(), "等待TF变换 (map -> base_link)...");
    rclcpp::Rate init_rate(10);
    int wait_count = 0;
    double current_x, current_y, current_yaw;
    
    while (rclcpp::ok() && wait_count < 50)
    {
      if (getCurrentPose(current_x, current_y, current_yaw))
      {
        RCLCPP_INFO(this->get_logger(), "TF变换可用，当前位置: x=%.2f, y=%.2f, yaw=%.2f", 
                    current_x, current_y, current_yaw);
        break;
      }
      init_rate.sleep();
      wait_count++;
    }
    
    if (wait_count >= 50)
    {
      result->success = false;
      result->message = "未接收到定位信息 (TF变换不可用)";
      goal_handle->abort(result);
      RCLCPP_ERROR(this->get_logger(), "导航失败: 未接收到TF变换 (map -> base_link)");
      return;
    }
    
    // 获取目标位置和姿态
    double target_x = goal->target_pose.pose.position.x;
    double target_y = goal->target_pose.pose.position.y;
    double target_yaw = getYawFromQuaternion(goal->target_pose.pose.orientation);
    
    RCLCPP_INFO(this->get_logger(), "目标位姿: x=%.2f, y=%.2f, yaw=%.2f", 
                target_x, target_y, target_yaw);
    
    rclcpp::Rate loop_rate(20); // 20 Hz
    auto cmd_vel = geometry_msgs::msg::Twist();
    
    // 外层循环：循环执行阶段1和阶段2，直到到达目标位置
    int phase_cycle_count = 0;
    while (rclcpp::ok())
    {
      phase_cycle_count++;
      
      // 检查是否已到达目标位置（可能在上一次循环中已经接近）
      if (!getCurrentPose(current_x, current_y, current_yaw))
      {
        loop_rate.sleep();
        continue;
      }
      
      double distance = calculateDistance(current_x, current_y, target_x, target_y);
      if (distance < position_tolerance_)
      {
        stopRobot();
        RCLCPP_INFO(this->get_logger(), "已到达目标位置，距离: %.3f m", distance);
        break;
      }
      
      // ========== 阶段1: 旋转朝向目标位置 ==========
      RCLCPP_INFO(this->get_logger(), "阶段1: 调整朝向目标位置 (周期 %d)", phase_cycle_count);
      
      // 初始化自适应角速度控制变量
      double current_angular_speed = angular_speed_initial_;
      int stagnation_count = 0;
      double prev_angle_error = 1000.0; // 初始化为一个大值
      int rotation_feedback_counter = 0;  // 用于控制日志输出频率
      
      // 转向启动序列：先给向前冲量0.5m/s持续0.1s，然后纯转向
      bool rotation_started = false;
      
      while (rclcpp::ok())
      {
        if (goal_handle->is_canceling())
        {
          stopRobot();
          result->success = false;
          result->message = "导航被取消";
          goal_handle->canceled(result);
          RCLCPP_INFO(this->get_logger(), "导航任务已取消");
          return;
        }
        
        // 从TF获取当前位姿
        if (!getCurrentPose(current_x, current_y, current_yaw))
        {
          loop_rate.sleep();
          continue;
        }
        
        // 计算朝向目标的角度
        double angle_to_target = calculateTargetAngle(current_x, current_y, target_x, target_y);
        double angle_diff = normalizeAngle(angle_to_target - current_yaw);
        
        // 自适应调整角速度并检测停滞
        if (prev_angle_error < 999.0) // 确保不是第一次迭代
        {
          // 计算角度变化量
          double angle_change = std::abs(angle_diff) - std::abs(prev_angle_error);
          
          // 如果角度变化小于阈值，认为进展缓慢
          if (std::abs(angle_change) < angle_change_threshold_ && rotation_started)
          {
            stagnation_count++;
            
            // 如果停滞次数超过阈值，增加角速度并重新执行向前冲量
            if (stagnation_count >= stagnation_count_threshold_)
            {
              current_angular_speed = std::min(current_angular_speed + angular_speed_increment_, 
                                               angular_speed_max_);
              RCLCPP_WARN(this->get_logger(), 
                          "检测到角度停滞，重新执行向前冲量并增加角速度至: %.2f rad/s", 
                          current_angular_speed);
              rotation_started = false;  // 重置标志，重新执行启动序列
              stagnation_count = 0; // 重置计数器
            }
          }
          else
          {
            // 角度变化正常，重置计数器
            stagnation_count = 0;
          }
        }
        prev_angle_error = angle_diff;
        
        // 发布反馈
        feedback->distance_remaining = calculateDistance(current_x, current_y, target_x, target_y);
        feedback->current_pose.pose.position.x = current_x;
        feedback->current_pose.pose.position.y = current_y;
        feedback->current_pose.header.frame_id = "map";
        feedback->current_pose.header.stamp = this->now();
        goal_handle->publish_feedback(feedback);
        
        // 每10次循环（约0.5秒）输出一次角度信息
        if (rotation_feedback_counter % 10 == 0)
        {
          RCLCPP_INFO(this->get_logger(), 
                      "调整朝向 - 角度误差: %.3f rad (%.1f度), 角速度: %.2f rad/s",
                      angle_diff, angle_diff * 180.0 / M_PI, current_angular_speed);
        }
        rotation_feedback_counter++;
        
        if (std::abs(angle_diff) < angle_tolerance_)
        {
          stopRobot();
          RCLCPP_INFO(this->get_logger(), "已朝向目标，角度误差: %.3f rad (%.2f度)", 
                      angle_diff, angle_diff * 180.0 / M_PI);
          break;
        }
        
        // 旋转控制 - 两阶段启动序列
        if (!rotation_started)
        {
          // 阶段1: 先给一个向前的线速度冲量（2个循环周期，0.1秒）
          cmd_vel.linear.x = 0.5;
          cmd_vel.angular.z = 0.0;
          cmd_vel_pub_->publish(cmd_vel);
          loop_rate.sleep();
          
          cmd_vel.linear.x = 0.5;
          cmd_vel.angular.z = 0.0;
          cmd_vel_pub_->publish(cmd_vel);
          loop_rate.sleep();
          
          // 阶段2: 取消线速度，开始纯转向
          rotation_started = true;
        }
        
        // 纯转向控制 - 使用自适应角速度
        cmd_vel.linear.x = 0.0;
        cmd_vel.angular.z = (angle_diff > 0) ? current_angular_speed : -current_angular_speed;
        cmd_vel_pub_->publish(cmd_vel);
        
        loop_rate.sleep();
      }
      
      // ========== 阶段2: 直行到目标位置 ==========
      RCLCPP_INFO(this->get_logger(), "阶段2: 直行到目标位置 (周期 %d)", phase_cycle_count);
      bool need_reorient = false;  // 是否需要重新调整方向
      int forward_feedback_counter = 0;  // 用于控制日志输出频率
      
      while (rclcpp::ok())
      {
        if (goal_handle->is_canceling())
        {
          stopRobot();
          result->success = false;
          result->message = "导航被取消";
          goal_handle->canceled(result);
          RCLCPP_INFO(this->get_logger(), "导航任务已取消");
          return;
        }
        
        // 从TF获取当前位姿
        if (!getCurrentPose(current_x, current_y, current_yaw))
        {
          loop_rate.sleep();
          continue;
        }
        
        double distance = calculateDistance(current_x, current_y, target_x, target_y);
        double angle_to_target = calculateTargetAngle(current_x, current_y, target_x, target_y);
        double angle_diff = normalizeAngle(angle_to_target - current_yaw);
        
        // 发布反馈
        feedback->distance_remaining = distance;
        feedback->current_pose.pose.position.x = current_x;
        feedback->current_pose.pose.position.y = current_y;
        feedback->current_pose.header.frame_id = "map";
        feedback->current_pose.header.stamp = this->now();
        goal_handle->publish_feedback(feedback);
        
        // 检查是否到达目标位置
        if (distance < position_tolerance_)
        {
          stopRobot();
          RCLCPP_INFO(this->get_logger(), "已到达目标位置，距离: %.3f m", distance);
          break;
        }
        
        // 检查角度偏差 - 简化为两种模式
        if (std::abs(angle_diff) > angle_deviation_threshold_)
        {
          // 角度偏差过大，返回阶段1重新调整
          stopRobot();
          RCLCPP_WARN(this->get_logger(), 
                      "角度偏差过大: %.3f rad (%.1f度) > 阈值 %.3f rad (%.1f度)，返回阶段1重新调整",
                      angle_diff, angle_diff * 180.0 / M_PI,
                      angle_deviation_threshold_, angle_deviation_threshold_ * 180.0 / M_PI);
          need_reorient = true;
          break;
        }
        
        // 角度偏差在可接受范围内，边直行边矫正
        cmd_vel.linear.x = linear_speed_;
        
        // 使用比例控制：角速度 = 增益系数 × 角度偏差
        cmd_vel.angular.z = correction_angular_gain_ * angle_diff;
        
        // 限制最大矫正角速度，避免过度矫正
        double max_correction_angular = 0.4; // 最大矫正角速度
        if (std::abs(cmd_vel.angular.z) > max_correction_angular)
        {
          cmd_vel.angular.z = (cmd_vel.angular.z > 0) ? max_correction_angular : -max_correction_angular;
        }
        
        // 每10次循环输出一次状态信息
        if (forward_feedback_counter % 10 == 0)
        {
          if (std::abs(angle_diff) > 0.02) // 角度偏差大于0.02 rad时输出矫正信息
          {
            RCLCPP_INFO(this->get_logger(), 
                        "边行边矫正 - 距离: %.2f m, 角度偏差: %.3f rad (%.1f度), 矫正角速度: %.2f rad/s",
                        distance, angle_diff, angle_diff * 180.0 / M_PI, cmd_vel.angular.z);
          }
          else
          {
            RCLCPP_INFO(this->get_logger(), 
                        "直行前进 - 距离: %.2f m, 角度偏差: %.3f rad (%.1f度)",
                        distance, angle_diff, angle_diff * 180.0 / M_PI);
          }
        }
        
        forward_feedback_counter++;
        cmd_vel_pub_->publish(cmd_vel);
        
        loop_rate.sleep();
      }
      
      // 如果不需要重新调整方向，说明已经到达目标位置，跳出外层循环
      if (!need_reorient)
      {
        break;
      }
    }
    
    // 阶段3: 调整到目标姿态
    RCLCPP_INFO(this->get_logger(), "阶段3: 调整到目标姿态");
    RCLCPP_INFO(this->get_logger(), "目标yaw: %.3f rad (%.1f度)", target_yaw, target_yaw * 180.0 / M_PI);
    
    // 初始化自适应角速度控制变量
    double current_angular_speed = angular_speed_initial_;
    int stagnation_count = 0;
    double prev_angle_error = 1000.0; // 初始化为一个大值
    int angle_feedback_counter = 0;  // 用于控制日志输出频率
    
    // 转向启动序列：先给向前冲量0.5m/s持续0.1s，然后纯转向
    bool rotation_started = false;
    
    while (rclcpp::ok())
    {
      if (goal_handle->is_canceling())
      {
        stopRobot();
        result->success = false;
        result->message = "导航被取消";
        goal_handle->canceled(result);
        RCLCPP_INFO(this->get_logger(), "导航任务已取消");
        return;
      }
      
      // 从TF获取当前位姿
      if (!getCurrentPose(current_x, current_y, current_yaw))
      {
        loop_rate.sleep();
        continue;
      }
      
      double angle_diff = normalizeAngle(target_yaw - current_yaw);
      
      // 自适应调整角速度并检测停滞
      if (prev_angle_error < 999.0) // 确保不是第一次迭代
      {
        // 计算角度变化量
        double angle_change = std::abs(angle_diff) - std::abs(prev_angle_error);
        
        // 如果角度变化小于阈值，认为进展缓慢
        if (std::abs(angle_change) < angle_change_threshold_ && rotation_started)
        {
          stagnation_count++;
          
          // 如果停滞次数超过阈值，增加角速度并重新执行向前冲量
          if (stagnation_count >= stagnation_count_threshold_)
          {
            current_angular_speed = std::min(current_angular_speed + angular_speed_increment_, 
                                             angular_speed_max_);
            RCLCPP_WARN(this->get_logger(), 
                        "检测到角度停滞，重新执行向前冲量并增加角速度至: %.2f rad/s", 
                        current_angular_speed);
            rotation_started = false;  // 重置标志，重新执行启动序列
            stagnation_count = 0; // 重置计数器
          }
        }
        else
        {
          // 角度变化正常，重置计数器
          stagnation_count = 0;
        }
      }
      prev_angle_error = angle_diff;
      
      // 发布反馈
      feedback->distance_remaining = 0.0;
      feedback->current_pose.pose.position.x = current_x;
      feedback->current_pose.pose.position.y = current_y;
      feedback->current_pose.header.frame_id = "map";
      feedback->current_pose.header.stamp = this->now();
      goal_handle->publish_feedback(feedback);
      
      // 每10次循环（约0.5秒）输出一次角度信息
      if (angle_feedback_counter % 10 == 0)
      {
        RCLCPP_INFO(this->get_logger(), 
                    "姿态调整中 - 当前yaw: %.3f rad (%.1f度), 目标yaw: %.3f rad (%.1f度), 误差: %.3f rad (%.1f度), 角速度: %.2f rad/s",
                    current_yaw, current_yaw * 180.0 / M_PI,
                    target_yaw, target_yaw * 180.0 / M_PI,
                    angle_diff, angle_diff * 180.0 / M_PI,
                    current_angular_speed);
      }
      angle_feedback_counter++;
      
      if (std::abs(angle_diff) < angle_tolerance_)
      {
        stopRobot();
        RCLCPP_INFO(this->get_logger(), "已调整到目标姿态，角度误差: %.3f rad (%.2f度)", 
                    angle_diff, angle_diff * 180.0 / M_PI);
        break;
      }
      
      // 旋转控制 - 两阶段启动序列
      if (!rotation_started)
      {
        // 阶段1: 先给一个向前的线速度冲量（2个循环周期，0.1秒）
        cmd_vel.linear.x = 0.5;
        cmd_vel.angular.z = 0.0;
        cmd_vel_pub_->publish(cmd_vel);
        loop_rate.sleep();
        
        cmd_vel.linear.x = 0.5;
        cmd_vel.angular.z = 0.0;
        cmd_vel_pub_->publish(cmd_vel);
        loop_rate.sleep();
        
        // 阶段2: 取消线速度，开始纯转向
        rotation_started = true;
      }
      
      // 纯转向控制 - 使用自适应角速度
      cmd_vel.linear.x = 0.0;
      cmd_vel.angular.z = (angle_diff > 0) ? current_angular_speed : -current_angular_speed;
      cmd_vel_pub_->publish(cmd_vel);
      
      loop_rate.sleep();
    }
    
    // 检查任务是否完成
    if (rclcpp::ok())
    {
      result->success = true;
      result->message = "成功到达目标位姿";
      goal_handle->succeed(result);
      RCLCPP_INFO(this->get_logger(), "导航任务成功完成!");
      RCLCPP_INFO(this->get_logger(), "目标位姿: x=%.2f, y=%.2f, yaw=%.2f", 
                  target_x, target_y, target_yaw);
    }
  }
  
  // 停止机器人
  void stopRobot()
  {
    auto cmd_vel = geometry_msgs::msg::Twist();
    cmd_vel.linear.x = 0.0;
    cmd_vel.angular.z = 0.0;
    cmd_vel_pub_->publish(cmd_vel);
  }
};

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<FruitNav>();
  
  // 使用MultiThreadedExecutor以支持动作服务器的并发执行
  rclcpp::executors::MultiThreadedExecutor executor;
  executor.add_node(node);
  executor.spin();
  
  rclcpp::shutdown();
  return 0;
}
